#! /usr/bin/env python3
# -*- coding: utf-8 -*-

from math import pi
import time

from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
from moveit.task_constructor import core, stages

import rclcpp

rclcpp.init()
node = rclcpp.Node("mtc_tutorial")

group = "panda_arm"

# Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath()
jointspace = core.JointInterpolationPlanner()

task = core.Task()
task.name = "cartesian"
task.loadRobotModel(node)

# start from current robot state
task.add(stages.CurrentState("current state"))

# 同时执行x+0.2, y-0.3, rz+45°
move = stages.MoveRelative("combined x+y+rz", cartesian)
move.group = group
header = Header(frame_id="world")
move.setDirection(TwistStamped(
    header=header, 
    twist=Twist(
        linear=Vector3(x=0.2, y=-0.3, z=0.0),    # 线速度
        angular=Vector3(x=0.0, y=0.0, z=pi/4)    # 角速度
    )
))
task.add(move)

# moveTo named posture, using joint-space interplation
move = stages.MoveTo("moveTo ready", jointspace)
move.group = group
move.setGoal("ready")
task.add(move)

if task.plan():
    task.publish(task.solutions[0])
time.sleep(100)
